/********************************************************************************
 * Copyright 2013 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWSIM_CONTACTS_CONTACT_HPP_
#define RWSIM_CONTACTS_CONTACT_HPP_

/**
 * @file rwsim/contacts/Contact.hpp
 *
 * \copydoc rwsim::contacts::Contact
 */

#include <rw/common/Serializable.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/core/Ptr.hpp>

namespace rw { namespace kinematics { class Frame; } }
namespace rw { namespace models { class WorkCell; } }

namespace rwsim {
namespace contacts {

class ContactModel;

//! @addtogroup rwsim_contacts

//! @{
/**
 * @brief A common interface for the most important data for a contact.
 *
 * A contact is given by the two points in contact, a normal, and a depth.
 * Furthermore the contact is generated by a contact strategy, which will use two internal contact models for the geometries in contact.
 * The relative transformation between these contact models is also used to define the contact.
 */
class Contact: public rw::common::Serializable {
public:
	/**
	 * @brief Create new non-initialized contact.
	 */
	Contact();

	/**
	 * @brief Destruct contact.
	 */
	virtual ~Contact();

	/**
	 * @brief Get the contact model for the first object.
	 *
	 * @return contact model specific to the strategy that created the contact.
	 */
	rw::core::Ptr<ContactModel> getModelA() const;

	/**
	 * @brief Get the contact model for the second object.
	 *
	 * @return contact model specific to the strategy that created the contact.
	 */
	rw::core::Ptr<ContactModel> getModelB() const;

	/**
	 * @brief Get the frame for the first object.
	 *
	 * @return pointer to the frame.
	 */
	const rw::kinematics::Frame* getFrameA() const;

	/**
	 * @brief Get the frame for the second object.
	 *
	 * @return pointer to the frame.
	 */
	const rw::kinematics::Frame* getFrameB() const;

	/**
	 * @brief Get the name of the first object.
	 *
	 * @return the name.
	 */
	const std::string& getNameA() const;

	/**
	 * @brief Get the name of the second object.
	 *
	 * @return the name.
	 */
	const std::string& getNameB() const;

	/**
	 * @brief The relative transform between the two contact models.
	 *
	 * @return the relative transform.
	 */
	rw::math::Transform3D<> aTb() const;

	/**
	 * @brief The contact point on the first object.
	 *
	 * @return the contact point.
	 */
	rw::math::Vector3D<> getPointA() const;

	/**
	 * @brief The contact point on the second object.
	 *
	 * @return the contact point.
	 */
	rw::math::Vector3D<> getPointB() const;

	/**
	 * @brief The contact normal from the first object towards the second object.
	 *
	 * @return the contact normal.
	 */
	rw::math::Vector3D<> getNormal() const;

	/**
	 * @brief The penetration depth between the two objects.
	 *
	 * @return the penetration depth, positive if penetrating, negative if separated.
	 */
	double getDepth() const;

	/**
	 * @brief Set the contact model for the first object.
	 *
	 * @param modelA [in] contact model specific for the contact strategy that generates the contact.
	 */
	void setModelA(rw::core::Ptr<ContactModel> modelA);

	/**
	 * @brief Set the contact model for the second object.
	 *
	 * @param modelB [in] contact model specific for the contact strategy that generates the contact.
	 */
	void setModelB(rw::core::Ptr<ContactModel> modelB);

	/**
	 * @brief Set the frame for the first object.
	 *
	 * @param frame [in] a pointer to the frame.
	 */
	void setFrameA(const rw::kinematics::Frame* frame);

	/**
	 * @brief Set the frame for the second object.
	 *
	 * @param frame [in] a pointer to the frame.
	 */
	void setFrameB(const rw::kinematics::Frame* frame);

	/**
	 * @brief Set the name of the first object.
	 * @note This will remove the pointer to frame A.
	 * @param name [in] the name.
	 */
	void setNameA(const std::string& name);

	/**
	 * @brief Set the name of the second object.
	 * @note This will remove the pointer to frame B.
	 * @param name [in] the name.
	 */
	void setNameB(const std::string& name);

	/**
	 * @brief Try to set the frame pointers by looking up names in workcell.
	 *
	 * Note that frame pointers will only be set if both frames can be found.
	 * The function returns false if no change has been done.
	 *
	 * @param wc [in] the workcell.
	 * @return true if both frames found, or false otherwise.
	 */
	bool setFrames(const rw::models::WorkCell& wc);

	/**
	 * @brief Set the relative transform from the first object to the second object.
	 *
	 * @param aTb [in] the relative transform.
	 */
	void setTransform(rw::math::Transform3D<> aTb);

	/**
	 * @brief Set the contact point on the first object.
	 *
	 * @param pointA [in] contact point.
	 */
	void setPointA(rw::math::Vector3D<> pointA);

	/**
	 * @brief Set the contact point on the second object.
	 *
	 * @param pointB [in] contact point.
	 */
	void setPointB(rw::math::Vector3D<> pointB);

	/**
	 * @brief Set the contact points on both objects at the same time.
	 *
	 * @param pointA [in] contact point on A.
	 * @param pointB [in] contact point on B.
	 */
	void setPoints(rw::math::Vector3D<> pointA, rw::math::Vector3D<> pointB);

	/**
	 * @brief Set the normal from the first object to the second.
	 *
	 * @param normal [in] contact normal.
	 */
	void setNormal(rw::math::Vector3D<> normal);

	/**
	 * @brief Calculate penetration depth automatically.
	 *
	 * Will calculate the penetration depth from the contact points and the normal.
	 */
	void setDepth();

	/**
	 * @brief Set the penetation depth.
	 *
	 * @param depth [in] penetration depth. Positive if penetrating, negative if separated.
	 */
	void setDepth(double depth);

	/**
	 * @brief Check if the contact is exactly the same as other contact.
	 * @param b [in] the contact to compare with.
	 * @return true if equal, false if not.
	 */
	bool operator==(const Contact& b) const;

	/**
	 * @brief Check if the contact is different from other contact.
	 * @param b [in] the contact to compare with.
	 * @return true if different, false if equal.
	 */
	bool operator!=(const Contact& b) const;

	//! @copydoc rw::common::Serializable::read
	virtual void read(class rw::common::InputArchive& iarchive, const std::string& id);

	//! @copydoc rw::common::Serializable::write
	virtual void write(class rw::common::OutputArchive& oarchive, const std::string& id) const;

private:
	rw::core::Ptr<ContactModel> _a, _b;
	const rw::kinematics::Frame * _frameA;
	const rw::kinematics::Frame * _frameB;
	std::string _nameA;
	std::string _nameB;
	rw::math::Transform3D<> _aTb;
	rw::math::Vector3D<> _pointA, _pointB;
	rw::math::Vector3D<> _normal;
	double _depth;
};


/**
 * @brief Stream operator.
 * @param out [in/out] the stream to write to.
 * @param contact [in] the contact to print.
 * @return the same ostream as out parameter.
 */
std::ostream& operator<<(std::ostream& out, const Contact& contact);

//! @}
} /* namespace contacts */
} /* namespace rwsim */
#endif /* RWSIM_CONTACTS_CONTACT_HPP_ */
